import numpy as np
import trimesh
from numpy import linalg as LA


def get_parameter(clamp):
    final_dict = {}
    for arg in clamp.args:
        first_index = arg.find('__')
        element_name = arg[:first_index]
        element_arg = arg[first_index + 2:]
        element = clamp.elements[element_name]['instance']
        element.args[element_arg]['v'] = clamp.args[arg]['v']


def get_catia(clamp):
    refer_catia = clamp.obj['clamp_points'][0][0]
    refer_body = refer_catia.split('/')[0]
    refer_point = clamp.get_point(refer_catia)
    refer_catia_body = None
    for e in clamp.obj['elements']:
        if e['name'] == refer_body:
            refer_catia_body = e['category']
            break  # break
    return refer_point, refer_catia_body


def get_distance(p1, p2):
    return ((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2 + (p1[2] - p2[2]) ** 2) ** 0.5


def get_area(p1, p2, p3):
    # p[x, y, z]
    a12 = get_distance(p1, p2)
    a13 = get_distance(p1, p3)
    a23 = get_distance(p2, p3)
    p = (a12 + a13 + a23) / 2
    area = (p * (p - a12) * (p - a13) * (p - a23)) ** 0.5
    return area


def is_inside(p1, p2, p3, pt, tol=1):
    t1 = [pt, p1, p2]
    t2 = [pt, p1, p3]
    t3 = [pt, p2, p3]
    area = get_area(p1, p2, p3)
    if abs(area - get_area(t1[0], t1[1], t1[2]) - get_area(t2[0], t2[1], t2[2]) - get_area(t3[0], t3[1], t3[2])) < tol:
        return abs(area - get_area(t1[0], t1[1], t1[2]) - get_area(t2[0], t2[1], t2[2]) - get_area(t3[0], t3[1], t3[2]))
    return 0


def get_normal(p1, p2, p3):
    a = (p2[1] - p1[1]) * (p3[2] - p1[2]) - (p2[2] - p1[2]) * (p3[1] - p1[1])
    b = (p2[2] - p1[2]) * (p3[0] - p1[0]) - (p2[0] - p1[0]) * (p3[2] - p1[2])
    c = (p2[0] - p1[0]) * (p3[1] - p1[1]) - (p2[1] - p1[1]) * (p3[0] - p1[0])
    return [a, b, c]


def from_point_get_normal(mesh, point):
    tri = mesh.triangles
    ans1 = []
    ans2 = []
    for i in tri:
        temp = is_inside(i[0], i[1], i[2], point)
        if temp:
            ans1.append(temp)
            ans2.append(i)
    k = ans2[ans1.index(min(ans1))]
    nor = get_normal(k[0], k[1], k[2])
    nor = np.array(nor)
    nor = nor / LA.norm(nor)
    return nor


# 获取夹具点的坐标
def get_clamp_point(clamp=None):
    point = []
    clamp_point = clamp.obj.get('clamp_points', [])
    for i in range(len(clamp_point)):
        for j in range(len(clamp_point[i])):
            point.append(clamp.get_point(clamp_point[i][j]))
    # print(point)
    points = np.array(point).reshape(len(clamp_point), len(clamp_point[0]), 3)
    # print("夹紧点的坐标为：", points)
    return points


# 获取夹具点对应的法线
def get_clamp_point_normal(clamp):
    points = []
    points_normals = []
    clamp_mesh = clamp.get_mesh()
    clamp_point = clamp.obj.get('clamp_points', [])
    for i in range(len(clamp_point)):
        for j in range(len(clamp_point[i])):
            cp = clamp.get_point(clamp_point[i][j])
            normal = from_point_get_normal(clamp_mesh, cp)

            points.append(clamp.get_point(clamp_point[i][j]))
            points_normals.append(-normal)
    print(points)
    print(points_normals)


def point_point2plane(point, plane_origin, plane_normal):
    """
    :param point: 一个点
    :param plane: 一个面
    :return: 点到面垂直的点坐标
    """
    v, is_intersect, distance = trimesh.intersections.planes_lines([plane_origin], [plane_normal], [point],
                                                                   [plane_normal],
                                                                   return_distance=True,
                                                                   return_denom=False)
    return v[0]
